/*
 * sys_notify.c
 *
 *  Created on: Dec 4, 2023
 *      Author: boen
 */

#include <sys_notify.h>

static uint8_t rx_buffer[DEBUG_RX_BUFF_MAX];


/**
* @brief
* @param
* @retval
*/
int debug_init(void)
{
	usart2_init();
	return 0;
}

/**
  * @brief
  * @param
  * @retval
  */
int debug_get_msg(uint8_t buffer[DEBUG_RX_BUFF_MAX])
{
	memcpy(rx_buffer,buffer,DEBUG_RX_BUFF_MAX);
	return 0;
}


/**
  * @brief
  * @param
  * @retval
  */
float notify_get_rxd(uint8_t index)
{
	/* to do */
	uint8_t x = index*4;
	uint8_t byteArry[4];

	for(int i = APP_NULL; i < 4; i++)
	{
		byteArry[i] = rx_buffer[x+i];
	}

	return byte2float(byteArry);
	/* end */
}


/**
* @brief
* @param
* @retval
*/
static void notify_debug(void)
{
	switch(rx_buffer[0])
	{
		case 0:															// 取消调试输出
			break;
		case 1:
			if(xbot.state == SYS_FLAG_IDLE)
			{
				xbot.state = SYS_FLAG_PLAY;
				rx_buffer[0] = 0;
			}
			break;
		case 0x11:														// 机械臂关节角度输出
			XDEBUG("[notify]: arm servo[1]->%0.4f ,servo[2]->%0.4f ,servo[3]->%0.4f ,servo[4]->%0.4f \r\n",
					xbot.manipulator.servo[PHY_SERVO_1].fActPOS,
					xbot.manipulator.servo[PHY_SERVO_2].fActPOS,
					xbot.manipulator.servo[PHY_SERVO_3].fActPOS,
					xbot.manipulator.servo[PHY_SERVO_4].fActPOS
					);
			break;
		case 0x12:														// 机械臂位姿输出
			XDEBUG("[notify]: arm x:%f y%f z%f roll:%f pitch:%f yaw:%f\r\n",
					xbot.manipulator.pose.position.X,
					xbot.manipulator.pose.position.Y,
					xbot.manipulator.pose.position.Z,
					xbot.manipulator.pose.angular.X,
					xbot.manipulator.pose.angular.Y,
					xbot.manipulator.pose.angular.Z
					);
			break;
		case 0x13:														// 底盘电机速度输出
			XDEBUG("[notify]: mobile motor[1]->%0.4f ,motor[2]->%0.4f ,motor[3]->%0.4f ,motor[4]->%0.4f \r\n",
					xbot.mobile.motor[PHY_MOTOR_1].fActSPD,
					xbot.mobile.motor[PHY_MOTOR_2].fActSPD,
					xbot.mobile.motor[PHY_MOTOR_3].fActSPD,
					xbot.mobile.motor[PHY_MOTOR_4].fActSPD
					);
			break;
		case 0x14:														// 底盘里程计输出
			XDEBUG("[notify]: mobile pos: X->%0.4f ,Y->%0.4f ,Theta->%0.4f\r\n",
					xbot.mobile.pose.position.X,
					xbot.mobile.pose.position.Y,
					xbot.mobile.pose.angular.Z);
			break;
		case 0x15:														// 相机目标对象信息输出
			XDEBUG("[notify]: cam ColorDetect->[%f %f %f %f]\r\n",
					xbot.camera.color.target_x,
					xbot.camera.color.target_y,
					xbot.camera.color.target_w,
					xbot.camera.color.target_h
					);
			break;
		case 0x16:
			XDEBUG("[notify]: imu z:%f\r\n", xbot.mobile.pose.angular.Z);
			break;
		case 0x21:														// 测试X轴运动1米
			if(mobile_move_x(1.0))
			{
				rx_buffer[0] = 0;
				XDEBUG("[notify]: test end.\r\n");
			}
			break;
		case 0x22:														// 测试Y轴运动1米
			if(mobile_move_y(1.0))
			{
				rx_buffer[0] = 0;
				XDEBUG("[notify]: test end.\r\n");
			}
			break;
		case 0x23:														// 测试原地旋转90°
			if(mobile_rotation_z(1.5708f))
			{
				rx_buffer[0] = 0;
				XDEBUG("[notify]: test end.\r\n");
			}
			break;
		default:
			break;
	}
}


/**
* @brief Function implementing the sysNotify thread.
* @param argument: Not used
* @retval None
*/
/* USER CODE END Header_sys_Notify */
void sys_notify(void *argument)
{
	/* USER CODE BEGIN sys_Notify */
	debug_init();
	battery_init();
	display_init();
	camera_init();

	/* Infinite loop */
	for(;;)
	{
		battery_polling();												// 更新电池电量
		display_polling();												// 更新任务显示
		camera_polling();												// 更新相机数据反馈
		notify_debug();
		osDelay(NOTIFY_THREAD_DELAY);
	}
	battery_deinit();
	display_deinit();
	camera_deinit();
	/* USER CODE END sys_Notify */
}
